"""
main_simulink.py
2023.3.29           __by xutao
超平小车仿真模型
与树莓派通讯，包含车辆模型与环境变量
接收树莓派参数 thr,brake,steer
发送报文 xo,yo,ho,vo,model,thr,brake,steer,soc

2023.4.7
增加TCP/IP连接
新增仿真频率
"""
import sys
import traceback
import serial
import re
import time
import numpy as np
import car_dynamatic_model as car
import socket
import pygame

connect_model = 0  # 通讯类型 0.未链接到任何端口 1.串口 2.以太网 3.以上均可
# 串口通讯端口
pygame.init()
screen = pygame.display.set_mode((300, 300))
pygame.display.set_caption("窗口")
background = pygame.image.load("Background.jpg").convert()
try:
    print("try to connect serial")
    port = '/dev/cu.usbserial-14240'
    ser = serial.Serial(port, 115200, timeout=0.4)  # 建立与树莓派的串口通讯
    connect_model = 1
except:
    connect_model = 0
    print("Serial port connect failure")
    print("Please replace the link port")

# TCP通讯端口
# try:
#     print("try to connect Ethernet")
#     tcp_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)  # 创建tcp套接字
#     tcp_socket.connect(('169.254.205', 8080))
#     tcp_socket.send("have connected".encode('utf-8'))
#     if connect_model == 1:
#         connect_model = 3
#     else:
#         connect_model = 2
# except:
#     print("Ethernet communication cannot be connected")
#     print("Please replace the link port")

if connect_model == 0:
    print("No available ports were found")
    sys.exit()  # 未读到端口，退出程序

kk = re.compile(r'[-0-9.0-9]+')  # 编译正则表达式，读取树莓派指令

global thr
global brake
global steer
main_car = car.vehicle()


def return_pai(data):  # 反馈指令
    ser.flushOutput()
    print(data)
    ser.write(bytes(data, encoding='utf-8'))


def return_pai_2(data):
    print(data)
    tcp_socket.send(data)


def pai_input():  # 树莓派指令解析
    global thr
    global brake
    global steer
    ser.flushInput()
    pai_r = ser.readline()
    pai_order = pai_r.decode()[:-2]
    pai = kk.findall(pai_order)
    if pai_order != '':
        try:
            thr = float(pai[0])
            brake = float(pai[1])
            steer = float(pai[2])
            print(f"receive data:thr = {thr},brake = {brake}, steer = {steer}")
        except:
            thr = 0.0
            brake = 0.0
            steer = 0.0
            print("Error obtaining enough Raspberry PI information")
    else:
        thr = 0.0
        brake = 0.0
        steer = 0.0
        print("Error obtaining Raspberry PI information")


def pai_input_2():
    global thr
    global brake
    global steer
    tcp_socket.recv(1024)
    pai_order = tcp_socket[:-2]
    pai = kk.findall(pai_order)
    if pai_order != '':
        try:
            thr = float(pai[0])
            brake = float(pai[1])
            steer = float(pai[2])
            print(f"receive data:thr = {thr},brake = {brake}, steer = {steer}")
        except:
            thr = 0.0
            brake = 0.0
            steer = 0.0
            print("Error obtaining enough Raspberry PI information")
    else:
        thr = 0.0
        brake = 0.0
        steer = 0.0
        print("Error obtaining Raspberry PI information")


while True:
    pai_order = ''
    rece = ''
    if connect_model == 1 or connect_model == 3:
        # 读取串口指令
        ser.flushInput()
        pai_r = ser.readline()
        pai_order = pai_r.decode()
    if connect_model == 2 or connect_model == 3:
        # 读取以太网指令
        rece_data = tcp_socket.recv(1024)
        rece = rece_data.decode('utf-8')
    if pai_order[:5] == "Ready":
        connect_model = 1
        print("the car simulink is ready by serial communication")
        break
    elif rece[:5] == "Ready":
        connect_model = 2
        print("the car simulink is ready by Ethernet communication")
        break
    else:
        print("waiting!")
i = 0
while True:
    pygame.event.get()
    keys_pressed = pygame.key.get_pressed()
    if keys_pressed[pygame.K_2]:
        main_car.model = 2
    if keys_pressed[pygame.K_1]:
        main_car.model = 1
    if keys_pressed[pygame.K_0]:
        main_car.model = 0
    try:
        if main_car.model == 2:
            if connect_model == 1:
                if i % 3 == 0:
                    pai_input()
                main_car.update_control(thr, brake, steer)
                # time.sleep(0.01)  # 模拟单片机传输频率10Hz，考虑到串口传输延迟，适当降低间隔时间
                # print(main_car.back_data())
                if i % 3 == 0:
                    data = main_car.back_data()
                    return_pai(data)
                i += 1
            elif connect_model == 2:
                pai_input_2()
                main_car.update_control(thr, brake, steer)
                data = main_car.back_data()
                return_pai_2(data)
        elif main_car.model == 1 or main_car.model == 0:
            if i % 3 == 0:
                if keys_pressed[pygame.K_w] and keys_pressed[pygame.K_SPACE]:        # 前进控制
                    main_car.game_thr(1)
                elif keys_pressed[pygame.K_w]:
                    main_car.game_thr(2)
                else:
                    main_car.game_thr(0)
                if keys_pressed[pygame.K_a]:        # 转向控制
                    main_car.game_steer(-1)
                elif keys_pressed[pygame.K_d]:
                    main_car.game_steer(1)
                else:
                    main_car.game_steer(0)
                if keys_pressed[pygame.K_s]:        # 制动控制
                    main_car.game_brake()
                else:
                    main_car.game_br = 0.0
            main_car.update_control(main_car.game_th,main_car.game_br, main_car.game_st / 45)
            time.sleep(0.05)
            if i % 3 == 0:
                data = main_car.back_data()
                return_pai(data)
            i += 1
    except:
        time.sleep(0.1)
        traceback.print_exc()
        print("error")
